#ifndef PANGOLIN_VIEWER_VIEWER_H
#define PANGOLIN_VIEWER_VIEWER_H

#include "color_scheme.h"

#include "publisher/publisher.h"

#include <memory>
#include <mutex>

#include <pangolin/pangolin.h>

using namespace publisher;

namespace pangolin_viewer {

class viewer {
public:
    /**
     * Constructor
     */
    viewer(std::shared_ptr<Publisher> publisher_ptr = nullptr);

	~viewer();
    /**
     * Main loop for window refresh
     */
    void run();

    /**
     * Request to terminate the viewer
     * (NOTE: this function does not wait for terminate)
     */
    void request_terminate();

    /**
     * Check if the viewer is terminated or not
     * @return whether the viewer is terminated or not
     */
    bool is_terminated();

private:
    void draw_horizontal_grid();
    
    /**
     * Create menu panel
     */
    void create_menu_panel();

    /**
     * Follow to the specified camera pose
     * @param gl_cam_pose_wc
     */
    void follow_camera(const pangolin::OpenGlMatrix& gl_cam_pose_wc);

    /**
     * Get the current camera pose via the map publisher
     * @return
     */
    pangolin::OpenGlMatrix get_current_cam_pose();
    
    void draw_vehicle_poses();

    void draw_gt_poses();
    
    void draw_odometry_poses();

    void draw_all_poses();
 
    /**
     * Get and draw landmarks via the map publisher
     */
    void draw_landmarks();
    void draw_filter_landmarks();
    void draw_current_landmarks();

    void draw_submap_landmarks();
    void draw_submap_poses();   
     
    void draw_all_submap_poses();    

    void draw_trajectory_node_list();

    void draw_constraint_list();

    void draw_curr_submap();

	void draw_particle();
    /**
     * Draw a frustum of a camera
     * @param w
     */
    void draw_frustum(const float w) const;  

    /**
     * Draw a line between two 3D points
     */
    void draw_line(const float x1, const float y1, const float z1,
                   const float x2, const float y2, const float z2) const;    

    /**
     * Reset the states
     */
    void reset();

    /**
     * Check state transition
     */
    //void check_state_transition();

    const unsigned int interval_ms_;

    const float viewpoint_x_, viewpoint_y_, viewpoint_z_, viewpoint_f_;

    const float keyfrm_size_;
    const float keyfrm_line_width_;
    const float graph_line_width_;
    const float point_size_;
    const float camera_size_;
    const float camera_line_width_;

    const color_scheme cs_;

    // menu panel
    std::unique_ptr<pangolin::Var<bool>> menu_follow_camera_;
    std::unique_ptr<pangolin::Var<bool>> menu_grid_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_keyfrms_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_lms_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_local_map_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_graph_;
    std::unique_ptr<pangolin::Var<bool>> menu_mapping_mode_;
    std::unique_ptr<pangolin::Var<bool>> menu_loop_detection_mode_;
    std::unique_ptr<pangolin::Var<bool>> menu_pause_;
    std::unique_ptr<pangolin::Var<bool>> menu_reset_;
    std::unique_ptr<pangolin::Var<bool>> menu_terminate_;
    std::unique_ptr<pangolin::Var<float>> menu_frm_size_;
    std::unique_ptr<pangolin::Var<float>> menu_lm_size_;
    std::unique_ptr<pangolin::Var<bool>> menu_show_veh_pose;
    std::unique_ptr<pangolin::Var<bool>> menu_show_keyframe_veh_pose;

    std::unique_ptr<pangolin::Var<bool>> menu_show_gt_pose;
    std::unique_ptr<pangolin::Var<bool>> menu_show_keyframe_gt_pose;

    std::unique_ptr<pangolin::Var<bool>> menu_show_wheel_odometry;  
    std::unique_ptr<pangolin::Var<bool>> menu_show_predict_pose2d;  
    std::unique_ptr<pangolin::Var<bool>> menu_show_predict_pose3d;  

    std::unique_ptr<pangolin::Var<bool>> menu_show_current_landmark_;   

    std::unique_ptr<pangolin::Var<bool>> menu_show_filter_lms_;   

    std::unique_ptr<pangolin::Var<bool>> menu_show_submap_lms_;     
    std::unique_ptr<pangolin::Var<bool>> menu_show_submap_poses_;  

    std::unique_ptr<pangolin::Var<bool>> menu_show_all_submap_poses_;  
    std::unique_ptr<pangolin::Var<bool>> menu_show_current_submap_;  

    std::unique_ptr<pangolin::Var<bool>> menu_show_trajectory_node_;  

    std::unique_ptr<pangolin::Var<bool>> menu_show_constraint_list_;   

    std::unique_ptr<pangolin::Var<bool>> menu_show_all_poses_;     

    // camera renderer
    std::unique_ptr<pangolin::OpenGlRenderState> s_cam_;

    // current state
    bool follow_camera_;
    bool mapping_mode_;
    bool loop_detection_mode_;

    // viewer appearance
    std::string map_viewer_name_;
    std::string frame_viewer_name_;
    float map_viewer_width_;
    float map_viewer_height_;

    //-----------------------------------------
    // management for terminate process

    //! mutex for access to terminate procedure
    mutable std::mutex mtx_terminate_;
    // std::mutex mMutex;
    /**
     * Check if termination is requested or not
     * @return
     */
    //bool terminate_is_requested();

    /**
     * Raise the flag which indicates the main loop has been already terminated
     */
    //void terminate();

    //! flag which indicates termination is requested or not
    bool terminate_is_requested_;
    //! flag which indicates whether the main loop is terminated or not
    bool is_terminated_;

    // bool init_pose_status;

    //std::vector<Eigen::Vector3f> landmarks;

    // Eigen::Vector3d init_pose;
    
    // double distance;

    std::shared_ptr<Publisher> publisher_ptr_ = nullptr;

    bool localization_status_;

    int trajectory_id_;
};

inline void viewer::draw_line(const float x1, const float y1, const float z1,
                              const float x2, const float y2, const float z2) const {
    glVertex3f(x1, y1, z1);
    glVertex3f(x2, y2, z2);
}

} // namespace pangolin_viewer

#endif // PANGOLIN_VIEWER_VIEWER_H
